#include "LF04.h"
#include "zf_common_headfile.h"

// 基准速度
int BaseSpeed = 200;

float error = 0;
float last_error = 0;
volatile int zhijiao_flag = 0;  // 直角弯标志
volatile int zhijaio_count = 0; // 直角弯计数

void LF04_Init(void)
{
    // 初始化4个引脚为下拉输入
    gpio_init(DH1_PIN, GPI, GPIO_HIGH, GPI_PULL_DOWN);
    gpio_init(DH2_PIN, GPI, GPIO_HIGH, GPI_PULL_DOWN);
    gpio_init(DH3_PIN, GPI, GPIO_HIGH, GPI_PULL_DOWN);
    gpio_init(DH4_PIN, GPI, GPIO_HIGH, GPI_PULL_DOWN);
}

/*
// void Track_LF04(void)
// {
//     int DH_state = (DH1 << 3) | (DH2 << 2) | (DH3 << 1) | DH4; // 将传感器状态组合成一个整数
//     switch (DH_state)
//     {
//         case 0: // 0000 左转
//             TargetSpeed_L = 0;
//             TargetSpeed_R = 200;
// //            Wanshu++; // 增加弯道计数
//             // printf("十字路口\n");
//             break;
//         case 1: // 0001 左转
//             TargetSpeed_L = 0;
//             TargetSpeed_R = 200; 
//             // 增加弯道计数
// 		        zhijiao_flag = 1; // 设置直角弯标志
//         zhijaio_count++;
//             // printf("左大湾\n");
//             break;

//         case 8: // 1000 右大湾
// 			TargetSpeed_L = 200;
//             TargetSpeed_R = 0;
//             // 增加弯道计数
// 			        zhijiao_flag = 2; // 设置直角弯标志
//         zhijaio_count++;
//             break;
		
//         case 3: // 0011 左转
//             TargetSpeed_L = 0;
//             TargetSpeed_R = 200; 
//             // printf("左大湾\n");
//             break;

//         case 12: // 1100 右大湾
            
			
// 			TargetSpeed_L = 200;
//             TargetSpeed_R = 0;
//             // printf("右90度弯\n");
//             break;
//         case 7: // 0111 右转
// 			TargetSpeed_L = 80;
//             TargetSpeed_R = 130;
			
//             // printf("右90度弯\n");
		
//             // printf("右大弯\n");
			
//             break;
//         case 14: // 1110 左转
//             TargetSpeed_L = 130;
//             TargetSpeed_R =  80;
//             // printf("右90度弯\n");
//             // printf("左大弯\n");
//             break;
//         case 11: // 1011 左微调
//             TargetSpeed_L = 115;
//             TargetSpeed_R = 135;

			
//             // printf("左微调\n");
//             break;

//         case 13: // 1101 右微调
// 			TargetSpeed_L = 135;
//             TargetSpeed_R = 115;

	
//             // printf("右微调\n");
//             break;
//         case 9: // 1001 直行
//             TargetSpeed_L = 130;
//             TargetSpeed_R = 130;


//             // printf("直行\n");
//             break;

// //       case 15://1111 丢线情况
// ////		    
// //			TargetSpeed_L = -10;
// //            TargetSpeed_R = 180;
// //			Wanshu++; // 增加弯道计数
// //            // printf("右90度弯\n");
// //			is_delay = 1; 			
// //            break;
//     }
// }
*/

void Track_LF04(void)
{
    int DH_state = (DH1 << 3) | (DH2 << 2) | (DH3 << 1) | DH4; // 将传感器状态组合成一个整数
    switch (DH_state)
    {

    case 0: // 0000 十字路口 - 停止两秒且鸣笛后直线通过

        error = 0;
        break;

    case 1: // 0001 左直角弯

        error = -150;
        zhijiao_flag = 1; // 设置直角弯标志
        zhijaio_count++;
        break;

    case 8: // 1000 右直角弯

        error = 150;
        zhijiao_flag = 2; // 设置直角弯标志
        zhijaio_count++;
        break;

    case 7: // 0111 左大弯

        error = -100;
        break;

    case 14: // 1110 右大弯

        error = 100;
        break;

    case 11: // 1011 左微调

        error = -30;
        break;

    case 13: // 1101 右微调

        error = 30;
        break;

    case 9: // 1001 直行

        error = 0;
        break;
    case 15: // 1111 丢线情况
        if (zhijiao_flag == 1)
        {
            // 如果是左直角弯丢线，继续左转
            error = -150;
        }
        else if (zhijiao_flag == 2)
        {
            // 如果是右直角弯丢线，继续右转
            error = 150;
        }
        break;

    default:
        // 其他情况
        // error = 0;
        break;
    }
    // 位置式PID
    static float kp = 15, ki = 0, kd = 21;
    static float integral = 0;
    static float base_speed = 2500; // 可根据实际调整

    integral += error;
    float output = kp * error + ki * integral + kd * (error - last_error);

    last_error = error;

    // 计算左右电机PWM
    int left_pwm = base_speed + output;
    int right_pwm = base_speed - output;
    printf("pwmLRO:%d,%d,%f,%d\n", left_pwm, right_pwm, output, zhijaio_count); // 打印调试信息

    // 限幅
    if (left_pwm > PWM_DUTY_MAX - 2000)
        left_pwm = PWM_DUTY_MAX - 2000;
    if (left_pwm < -PWM_DUTY_MAX + 2000)
        left_pwm = -PWM_DUTY_MAX + 2000;
    if (right_pwm > PWM_DUTY_MAX - 2000)
        right_pwm = PWM_DUTY_MAX - 2000;
    if (right_pwm < -PWM_DUTY_MAX + 2000)
        right_pwm = -PWM_DUTY_MAX + 2000;
    // 确保PWM值在合理范围内

    // 输出到电机
    Motor_Out(left_pwm, right_pwm);
}

/*
// void Track_LF04(void) //实在不行用这套
// {
//     int DH_state = (DH1 << 3) | (DH2 << 2) | (DH3 << 1) | DH4; // 将传感器状态组合成一个整数
//     switch (DH_state)
//     {

//     case 0: // 0000 十字路口 - 停止两秒且鸣笛后直线通过

//         error = 0;
//         break;

//     case 1: // 0001 左直角弯

//         error = -150;
//         zhijiao_flag = 1; // 设置直角弯标志
//         zhijaio_count++;
//         break;

//     case 8: // 1000 右直角弯

//         error = 150;
//         zhijiao_flag = 2; // 设置直角弯标志
//         zhijaio_count++;
//         break;

//     case 7: // 0111 左大弯

//         error = -100;
//         break;

//     case 14: // 1110 右大弯

//         error = 100;
//         break;

//     case 11: // 1011 左微调

//         error = -30;
//         break;

//     case 13: // 1101 右微调

//         error = 30;
//         break;

//     case 9: // 1001 直行

//         error = 0;
//         break;
//     case 15: // 1111 丢线情况
//         if (zhijiao_flag == 1)
//         {
//             // 如果是左直角弯丢线，继续左转
//             error = -150;
//         }
//         else if (zhijiao_flag == 2)
//         {
//             // 如果是右直角弯丢线，继续右转
//             error = 150;
//         }
//         break;

//     default:
//         // 其他情况
//         // error = 0;
//         break;
//     }
//     // 位置式PID
//     static float kp = 15, ki = 0, kd = 21;
//     static float integral = 0;
//     static float base_speed = 2500; // 可根据实际调整

//     integral += error;
//     float output = kp * error + ki * integral + kd * (error - last_error);

//     last_error = error;

//     // 计算左右电机PWM
//     int left_pwm = base_speed + output;
//     int right_pwm = base_speed - output;
//     printf("pwmLRO:%d,%d,%f,%d\n", left_pwm, right_pwm, output, zhijaio_count); // 打印调试信息

//     // 限幅
//     if (left_pwm > PWM_DUTY_MAX - 2000)
//         left_pwm = PWM_DUTY_MAX - 2000;
//     if (left_pwm < -PWM_DUTY_MAX + 2000)
//         left_pwm = -PWM_DUTY_MAX + 2000;
//     if (right_pwm > PWM_DUTY_MAX - 2000)
//         right_pwm = PWM_DUTY_MAX - 2000;
//     if (right_pwm < -PWM_DUTY_MAX + 2000)
//         right_pwm = -PWM_DUTY_MAX + 2000;
//     // 确保PWM值在合理范围内

//     // 输出到电机
//     Motor_Out(left_pwm, right_pwm);
// }
*/

/*void Track_LF04(void)
{
    int DH_state = (DH1 << 3) | (DH2 << 2) | (DH3 << 1) | DH4; // 将传感器状态组合成一个整数
    switch (DH_state)
    {

    case 0: // 0000 十字路口 - 停止两秒且鸣笛后直线通过

        break;

    case 1: // 0001 左直角弯

        // error = -150;
        TargetSpeed_L = BaseSpeed + 150;
        TargetSpeed_R = BaseSpeed - 150;

        zhijiao_flag = 1; // 设置直角弯标志
        zhijaio_count++;
        break;

    case 8: // 1000 右直角弯

        // error = 150;
        TargetSpeed_L = BaseSpeed - 150;
        TargetSpeed_R = BaseSpeed + 150;

        zhijiao_flag = 2; // 设置直角弯标志
        zhijaio_count++;
        break;

    case 7: // 0111 左大弯

        // error = -100;
        TargetSpeed_L = BaseSpeed + 100;
        TargetSpeed_R = BaseSpeed - 100;
        break;

    case 14: // 1110 右大弯

        // error = 100;
        TargetSpeed_L = BaseSpeed - 100;
        TargetSpeed_R = BaseSpeed + 100;
        break;

    case 11: // 1011 左微调

        // error = -30;
        TargetSpeed_L = BaseSpeed + 30;
        TargetSpeed_R = BaseSpeed - 30;
        break;

    case 13: // 1101 右微调

        // error = 30;
        TargetSpeed_L = BaseSpeed - 30;
        TargetSpeed_R = BaseSpeed + 30;
        break;

    case 9: // 1001 直行

        // error = 0;
        TargetSpeed_L = BaseSpeed;
        TargetSpeed_R = BaseSpeed;
        break;
    case 15: // 1111 丢线情况
        if (zhijiao_flag == 1)
        {
            // 如果是左直角弯丢线，继续左转
            // error = -150;
            TargetSpeed_L = BaseSpeed + 150;
            TargetSpeed_R = BaseSpeed - 150;
        }
        else if (zhijiao_flag == 2)
        {
            // 如果是右直角弯丢线，继续右转
            // error = 150;
            TargetSpeed_L = BaseSpeed - 150;
            TargetSpeed_R = BaseSpeed + 150;
        }
        break;

    default:
        // 其他情况
        // error = 0;
        break;
    }

    // Motor_Out(Velocity_A(TargetSpeed_L, Encoder_Speed_L), Velocity_B(TargetSpeed_R, Encoder_Speed_R));
    // printf("pwmLRO:%d,%d,%d,%d\n", TargetSpeed_L, TargetSpeed_R, Encoder_Speed_L, Encoder_Speed_R); // 打印调试信息

    // // 位置式PID
    // static float kp = 15, ki = 0, kd = 21;
    // static float integral = 0;
    // static float base_speed = 2500; // 可根据实际调整

    // integral += error;
    // float output = kp * error + ki * integral + kd * (error - last_error);

    // last_error = error;

    // // 计算左右电机PWM
    // int left_pwm = base_speed + output;
    // int right_pwm = base_speed - output;
    // printf("pwmLRO:%d,%d,%f,%d\n", left_pwm, right_pwm, output, zhijaio_count); // 打印调试信息

    // // 限幅
    // if (left_pwm > PWM_DUTY_MAX - 2000)
    //     left_pwm = PWM_DUTY_MAX - 2000;
    // if (left_pwm < -PWM_DUTY_MAX + 2000)
    //     left_pwm = -PWM_DUTY_MAX + 2000;
    // if (right_pwm > PWM_DUTY_MAX - 2000)
    //     right_pwm = PWM_DUTY_MAX - 2000;
    // if (right_pwm < -PWM_DUTY_MAX + 2000)
    //     right_pwm = -PWM_DUTY_MAX + 2000;
    // // 确保PWM值在合理范围内

    // // 输出到电机
    // Motor_Out(left_pwm, right_pwm);
}
*/